#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include "BSplineInterpol.h"

using namespace PBVP;

const float BSplineInterpol::DEFAULT_BSI_EPSILON = 1.0e-9f;
float BSplineInterpol::TensorF[27];
float BSplineInterpol::TensorFx[27];
float BSplineInterpol::TensorFy[27]; 
float BSplineInterpol::TensorFz[27];
float BSplineInterpol::TensorFxx[27];
float BSplineInterpol::TensorFxy[27];
float BSplineInterpol::TensorFxz[27];
float BSplineInterpol::TensorFyy[27];
float BSplineInterpol::TensorFyz[27];
float BSplineInterpol::TensorFzz[27];

void BSplineInterpol::interpolCoeff1D(float *s, int size, float epsilon)
{
	//
	int   i, n, ni, ni1, K;
	double sum, z1, w1, w2;

	n = size + 1;
	z1 = sqrt(3.0) - 2.0;
	K = log(epsilon)/log(fabs(z1));
	//printf("K = %i\n", K);

	// compute initial value s(0)
	sum = 0.0;
	w2 = pow(z1, 2*n);
	if (n < K) {
		for (i = 1; i < n; i++){
			w1 = pow(z1, i);
			sum = sum + s[i-1]*(w1 - w2/w1);
		}
	} else {
		for (i = 1; i < n; i++){
			sum = (sum + s[n- i-1])*z1;
		}
	}
	sum = -sum/(1.0 - w2);


	// compute c^{+}
	n = size;
	s[0]  = s[0] + z1*sum;
	for (i = 1; i < n; i++) {
		s[i]  = s[i] + z1*s[i-1];
		// printf("cp[%i] = %e, %f \n", i, cp[i], z1);
	}

	// compute c^- 
	s[n-1] = -z1*s[n-1];
	for (i = 1; i < n; i++) {
		ni = n - i; 
		ni1 = ni - 1;
		s[ni1]  = z1*(s[ni] - s[ni1]);
	}

	for (i = 0; i < n; i++) {
		s[i]  = 6.0*s[i];
	}
}

void BSplineInterpol::interpolCoeff2D(float *s, int nx, int ny, float epsilon /* = DEFAULT_BSI_EPSILON */)
{
	float *d;
	int    i, l;

	d = (float *) malloc (ny*sizeof (float)); 

	// x-direction interpolation
	for (i = 0; i < ny; i++) {
		interpolCoeff1D(s+i*nx, nx, epsilon);
	}
	/*for (l = 0; l < ny; l++) {
		for (i = 0; i < nx; i++) {
			d[i] = s[i*ny + l];
		}
		interpolCoeff1D(d, nx, epsilon);

		for (i = 0; i < nx; i++) {
			s[i*ny + l] = d[i];
		}
	}*/

	// y-direction interpolation
	/*for (i = 0; i < nx; i++) {
		interpolCoeff1D(s+i*ny, ny, epsilon);
	}*/
	for (l = 0; l < nx; l++) {
		for (i = 0; i < ny; i++)
		{
			d[i] = s[i*nx + l];
		}
		interpolCoeff1D(d, ny, epsilon);

		for (i = 0; i < ny; i++)
		{
			s[i*nx + l] = d[i];
		}
	}

	free(d);
}

void BSplineInterpol::interpolCoeff3D(float *s, int nx, int ny, int nz, float epsilon /* = DEFAULT_BSI_EPSILON */)
{

	float *d;
	int    i, j, k, nxy;

	d = (float *) malloc (nz*sizeof (float));

	nxy = nx*ny;

	// z-direction interpolation
	for (i = 0; i < ny; i++) {
		for (j = 0; j < nx; j++) {
			int off = i*nx + j;
			for (k = 0; k < nz; k++) {
				d[k] = s[k*nxy + off];
			}		
			interpolCoeff1D(d, nx, epsilon);

			for (k = 0; k < nz; k++) {
				s[k*nxy + off] = d[k];
			}
		}
	}

	for (i = 0; i < nz; i++) {
		interpolCoeff2D(s + i*nxy, nx, ny, epsilon);
	}

	free(d);
}

void BSplineInterpol::partialDeriv12(float *c, float dx, float dy, float dz, int nx, int ny, int nz, int u, int v, int w, float *partials)
{
	float c27[27], indx, indy, indz;
	int  i;

	get27Coefficients(c, nx, ny, nz, u, v, w, c27);

	for (i = 0; i < 10; i++) {
		partials[i] = 0.0;
	}

	for (i = 0; i < 27; i++) {
		partials[0] = partials[0] + c27[i]*TensorF[i];

		partials[1] = partials[1] + c27[i]*TensorFx[i];
		partials[2] = partials[2] + c27[i]*TensorFy[i];
		partials[3] = partials[3] + c27[i]*TensorFz[i];

		partials[4] = partials[4] + c27[i]*TensorFxx[i];
		partials[5] = partials[5] + c27[i]*TensorFxy[i];
		partials[6] = partials[6] + c27[i]*TensorFxz[i];

		partials[7] = partials[7] + c27[i]*TensorFyy[i];
		partials[8] = partials[8] + c27[i]*TensorFyz[i];

		partials[9] = partials[9] + c27[i]*TensorFzz[i];

		//printf("i = %d   Coeff = %f,     Fxx = %f\n", i, c27[i], TensorFxx[i]);
	}      

	indx = 1.0/dx;
	indy = 1.0/dy;
	indz = 1.0/dz;

	//printf("indx = %f\n", partials[4]);

	partials[1] = partials[1] * indx;
	partials[2] = partials[2] * indy;
	partials[3] = partials[3] * indz;

	partials[4] = partials[4] * indx*indx;
	partials[5] = partials[5] * indx*indy;
	partials[6] = partials[6] * indx*indz;

	partials[7] = partials[7] * indy*indy;
	partials[8] = partials[8] * indy*indz;

	partials[9] = partials[9] * indz*indz;
}

void BSplineInterpol::computeTensor()
{
	float f[3], fx[3], fxx[3];

	f[0] = 1.0/6.0;
	f[1] = 2.0/3.0;
	f[2] = f[0];

	fx[0] = -0.5;
	fx[1] = 0.0;
	fx[2] = 0.5;

	fxx[0] = 1.0;
	fxx[1] = -2.0;
	fxx[2] = 1.0;

	Tensor_333(f, f, f, TensorF);

	Tensor_333(fx, f, f, TensorFx);
	Tensor_333(f, fx, f, TensorFy);
	Tensor_333(f, f, fx, TensorFz);

	Tensor_333(fxx,f, f, TensorFxx);
	Tensor_333(fx, fx,f, TensorFxy);
	Tensor_333(fx, f, fx,TensorFxz);

	Tensor_333(f, fxx,f,  TensorFyy);
	Tensor_333(f, fx, fx, TensorFyz);
	Tensor_333(f, f,  fxx,TensorFzz);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Private functions
////////////////////////////////////////////////////////////////////////////////////////////////////////
void BSplineInterpol::Tensor_333(float *xx, float *yy, float *zz, float *result)
{
	int i, j, k, l;

	l = 0;
	for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++) {
			for (k = 0; k < 3; k++) {
				result[l] = xx[i]*yy[j]*zz[k];
				l = l + 1;
			}
		}
	}  
}

float BSplineInterpol::getOneCoefficient(float *c, int nx, int ny, int nz, int u, int v, int w)
{
	float result;

	result = 0.0;
	if (( u  >= 0  && u < nx) &&
		( v  >= 0  && v < ny) &&
		( w  >= 0  && w < nz) ) {

			//result = c[(u*ny + v)*nz + w];
			result = c[(w*ny + v)*nx + u];
	}

	return(result);
}

void BSplineInterpol::get27Coefficients(float *c, int nx, int ny, int nz, int u, int v, int w, float *c27)
{
	c27[0] = getOneCoefficient(c, nx, ny, nz, u-1,v-1,w-1);
	c27[1] = getOneCoefficient(c, nx, ny, nz, u-1,v-1,w);
	c27[2] = getOneCoefficient(c, nx, ny, nz, u-1,v-1,w+1);

	c27[3] = getOneCoefficient(c, nx, ny, nz, u-1,v,w-1);
	c27[4] = getOneCoefficient(c, nx, ny, nz, u-1,v,w);
	c27[5] = getOneCoefficient(c, nx, ny, nz, u-1,v,w+1);

	c27[6] = getOneCoefficient(c, nx, ny, nz, u-1,v+1,w-1);
	c27[7] = getOneCoefficient(c, nx, ny, nz, u-1,v+1,w);
	c27[8] = getOneCoefficient(c, nx, ny, nz, u-1,v+1,w+1);

	c27[9]  = getOneCoefficient(c, nx, ny, nz, u,v-1,w-1);
	c27[10] = getOneCoefficient(c, nx, ny, nz, u,v-1,w);
	c27[11] = getOneCoefficient(c, nx, ny, nz, u,v-1,w+1);

	c27[12] = getOneCoefficient(c, nx, ny, nz, u,v,w-1);
	c27[13] = getOneCoefficient(c, nx, ny, nz, u,v,w);
	c27[14] = getOneCoefficient(c, nx, ny, nz, u,v,w+1);

	c27[15] = getOneCoefficient(c, nx, ny, nz, u,v+1,w-1);
	c27[16] = getOneCoefficient(c, nx, ny, nz, u,v+1,w);
	c27[17] = getOneCoefficient(c, nx, ny, nz, u,v+1,w+1);

	c27[18] = getOneCoefficient(c, nx, ny, nz, u+1,v-1,w-1);
	c27[19] = getOneCoefficient(c, nx, ny, nz, u+1,v-1,w);
	c27[20] = getOneCoefficient(c, nx, ny, nz, u+1,v-1,w+1);

	c27[21] = getOneCoefficient(c, nx, ny, nz, u+1,v,w-1);
	c27[22] = getOneCoefficient(c, nx, ny, nz, u+1,v,w);
	c27[23] = getOneCoefficient(c, nx, ny, nz, u+1,v,w+1);

	c27[24] = getOneCoefficient(c, nx, ny, nz, u+1,v+1,w-1);
	c27[25] = getOneCoefficient(c, nx, ny, nz, u+1,v+1,w);
	c27[26] = getOneCoefficient(c, nx, ny, nz, u+1,v+1,w+1);
}